/*
Copyright (c) 2012 Juan Mellado

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
 */

/*
References:
- "Iterative Pose Estimation using Coplanar Feature Points"
Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis
http://www.cfar.umd.edu/~daniel/daniel_papersfordownload/CoplanarPts.pdf
 */

var POS = POS || {};

POS.Posit = function (modelSize, focalLength) {
	this.objectPoints = this.buildModel(modelSize);
	this.focalLength = focalLength;

	this.objectVectors = [];
	this.objectNormal = [];
	this.objectMatrix = [[], [], []];

	this.init();
};

POS.Posit.prototype.buildModel = function (modelSize) {
	var half = modelSize / 2.0;

	return [
		[-half, half, 0.0],
		[half, half, 0.0],
		[half, -half, 0.0],
		[-half, -half, 0.0]];
};

POS.Posit.prototype.init = function () {
	var np = this.objectPoints.length,
	vectors = [],
	n = [],
	len = 0.0,
	row = 2,
	i;
	for (i = 0; i < np; ++i) {
		this.objectVectors[i] = [
			this.objectPoints[i][0] - this.objectPoints[0][0],
			this.objectPoints[i][1] - this.objectPoints[0][1],
			this.objectPoints[i][2] - this.objectPoints[0][2]
		];
		vectors[i] = [
			this.objectVectors[i][0],
			this.objectVectors[i][1],
			this.objectVectors[i][2]
		];
	}
	while (0.0 === len) {
		n[0] = this.objectVectors[1][1] * this.objectVectors[row][2] -
			this.objectVectors[1][2] * this.objectVectors[row][1];
		n[1] = this.objectVectors[1][2] * this.objectVectors[row][0] -
			this.objectVectors[1][0] * this.objectVectors[row][2];
		n[2] = this.objectVectors[1][0] * this.objectVectors[row][1] -
			this.objectVectors[1][1] * this.objectVectors[row][0];

		len = Math.sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);
		++row;
	}
	for (i = 0; i < 3; ++i) {
		this.objectNormal[i] = n[i] / len;
	}
	POS.pseudoInverse(vectors, np, this.objectMatrix);
};

POS.Posit.prototype.pose = function (imagePoints) {
	var posRotation1 = [[], [], []],
	posRotation2 = [[], [], []],
	posTranslation = [],
	rotation1 = [[], [], []],
	rotation2 = [[], [], []],
	translation1 = [],
	translation2 = [],
	error1,
	error2,
	valid1,
	valid2,
	i,
	j;

	this.pos(imagePoints, posRotation1, posRotation2, posTranslation);

	valid1 = this.isValid(posRotation1, posTranslation);
	if (valid1) {
		error1 = this.iterate(imagePoints, posRotation1, posTranslation, rotation1, translation1);
		// console.assert(error1)
	} else {
		error1 = {
			euclidean : -1.0,
			pixels : -1,
			maximum : -1.0
		};
	}

	valid2 = this.isValid(posRotation2, posTranslation);
	if (valid2) {
		error2 = this.iterate(imagePoints, posRotation2, posTranslation, rotation2, translation2);
		// console.assert(error2)
	} else {
		error2 = {
			euclidean : -1.0,
			pixels : -1,
			maximum : -1.0
		};
	}

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < 3; ++j) {
			if (valid1) {
				translation1[i] -= rotation1[i][j] * this.objectPoints[0][j];
			}
			if (valid2) {
				translation2[i] -= rotation2[i][j] * this.objectPoints[0][j];
			}
		}
	}

	// @jeromeetienne patch
	if (error1 === undefined || error2 === undefined)
		return null;

	return error1.euclidean < error2.euclidean ?
	new POS.Pose(error1.pixels, rotation1, translation1, error2.pixels, rotation2, translation2) :
	new POS.Pose(error2.pixels, rotation2, translation2, error1.pixels, rotation1, translation1);
};

POS.Posit.prototype.pos = function (imagePoints, rotation1, rotation2, translation) {
	var np = this.objectPoints.length,
	imageVectors = [],
	i0 = [],
	j0 = [],
	ivec = [],
	jvec = [],
	row1 = [],
	row2 = [],
	row3 = [],
	i0i0,
	j0j0,
	i0j0,
	delta,
	q,
	lambda,
	mu,
	scale,
	i,
	j;

	for (i = 0; i < np; ++i) {
		imageVectors[i] = [imagePoints[i].x - imagePoints[0].x,
			imagePoints[i].y - imagePoints[0].y];
	}

	//i0 and j0
	for (i = 0; i < 3; ++i) {
		i0[i] = 0.0;
		j0[i] = 0.0;
		for (j = 0; j < np; ++j) {
			i0[i] += this.objectMatrix[i][j] * imageVectors[j][0];
			j0[i] += this.objectMatrix[i][j] * imageVectors[j][1];
		}
	}

	i0i0 = i0[0] * i0[0] + i0[1] * i0[1] + i0[2] * i0[2];
	j0j0 = j0[0] * j0[0] + j0[1] * j0[1] + j0[2] * j0[2];
	i0j0 = i0[0] * j0[0] + i0[1] * j0[1] + i0[2] * j0[2];

	//Lambda and mu
	delta = (j0j0 - i0i0) * (j0j0 - i0i0) + 4.0 * (i0j0 * i0j0);

	if (j0j0 - i0i0 >= 0.0) {
		q = (j0j0 - i0i0 + Math.sqrt(delta)) / 2.0;
	} else {
		q = (j0j0 - i0i0 - Math.sqrt(delta)) / 2.0;
	}

	if (q >= 0.0) {
		lambda = Math.sqrt(q);
		if (0.0 === lambda) {
			mu = 0.0;
		} else {
			mu = -i0j0 / lambda;
		}
	} else {
		lambda = Math.sqrt( - (i0j0 * i0j0) / q);
		if (0.0 === lambda) {
			mu = Math.sqrt(i0i0 - j0j0);
		} else {
			mu = -i0j0 / lambda;
		}
	}

	//First rotation
	for (i = 0; i < 3; ++i) {
		ivec[i] = i0[i] + lambda * this.objectNormal[i];
		jvec[i] = j0[i] + mu * this.objectNormal[i];
	}

	scale = Math.sqrt(ivec[0] * ivec[0] + ivec[1] * ivec[1] + ivec[2] * ivec[2]);

	for (i = 0; i < 3; ++i) {
		row1[i] = ivec[i] / scale;
		row2[i] = jvec[i] / scale;
	}

	row3[0] = row1[1] * row2[2] - row1[2] * row2[1];
	row3[1] = row1[2] * row2[0] - row1[0] * row2[2];
	row3[2] = row1[0] * row2[1] - row1[1] * row2[0];

	for (i = 0; i < 3; ++i) {
		rotation1[0][i] = row1[i];
		rotation1[1][i] = row2[i];
		rotation1[2][i] = row3[i];
	}

	//Second rotation
	for (i = 0; i < 3; ++i) {
		ivec[i] = i0[i] - lambda * this.objectNormal[i];
		jvec[i] = j0[i] - mu * this.objectNormal[i];
	}

	for (i = 0; i < 3; ++i) {
		row1[i] = ivec[i] / scale;
		row2[i] = jvec[i] / scale;
	}

	row3[0] = row1[1] * row2[2] - row1[2] * row2[1];
	row3[1] = row1[2] * row2[0] - row1[0] * row2[2];
	row3[2] = row1[0] * row2[1] - row1[1] * row2[0];

	for (i = 0; i < 3; ++i) {
		rotation2[0][i] = row1[i];
		rotation2[1][i] = row2[i];
		rotation2[2][i] = row3[i];
	}

	//Translation
	translation[0] = imagePoints[0].x / scale;
	translation[1] = imagePoints[0].y / scale;
	translation[2] = this.focalLength / scale;
};

POS.Posit.prototype.isValid = function (rotation, translation) {
	var np = this.objectPoints.length,
	zmin = Infinity,
	i = 0,
	zi;

	for (; i < np; ++i) {
		zi = translation[2] +
			(rotation[2][0] * this.objectVectors[i][0] +
				rotation[2][1] * this.objectVectors[i][1] +
				rotation[2][2] * this.objectVectors[i][2]);
		if (zi < zmin) {
			zmin = zi;
		}
	}

	return zmin >= 0.0;
};

POS.Posit.prototype.iterate = function (imagePoints, posRotation, posTranslation, rotation, translation) {
	var np = this.objectPoints.length,
	oldSopImagePoints = [],
	sopImagePoints = [],
	rotation1 = [[], [], []],
	rotation2 = [[], [], []],
	translation1 = [],
	translation2 = [],
	converged = false,
	iteration = 0,
	oldImageDifference,
	imageDifference,
	factor,
	error,
	error1,
	error2,
	delta,
	i,
	j;

	for (i = 0; i < np; ++i) {
		oldSopImagePoints[i] = {
			x : imagePoints[i].x,
			y : imagePoints[i].y
		};
	}

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < 3; ++j) {
			rotation[i][j] = posRotation[i][j];
		}
		translation[i] = posTranslation[i];
	}

	for (i = 0; i < np; ++i) {
		factor = 0.0;
		for (j = 0; j < 3; ++j) {
			factor += this.objectVectors[i][j] * rotation[2][j] / translation[2];
		}
		sopImagePoints[i] = {
			x : (1.0 + factor) * imagePoints[i].x,
			y : (1.0 + factor) * imagePoints[i].y
		};
	}

	imageDifference = 0.0;

	for (i = 0; i < np; ++i) {
		imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x);
		imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y);
	}

	for (i = 0; i < 3; ++i) {
		translation1[i] = translation[i] -
			(rotation[i][0] * this.objectPoints[0][0] +
				rotation[i][1] * this.objectPoints[0][1] +
				rotation[i][2] * this.objectPoints[0][2]);
	}

	error1 = this.error(imagePoints, rotation, translation1);

	//Convergence
	converged = (0.0 === error1.pixels) || (imageDifference < 0.01);
	while (iteration++ < 100 && !converged) {

		for (i = 0; i < np; ++i) {
			oldSopImagePoints[i].x = sopImagePoints[i].x;
			oldSopImagePoints[i].y = sopImagePoints[i].y;
		}

		this.pos(sopImagePoints, rotation1, rotation2, translation);

		for (i = 0; i < 3; ++i) {
			translation1[i] = translation[i] -
				(rotation1[i][0] * this.objectPoints[0][0] +
					rotation1[i][1] * this.objectPoints[0][1] +
					rotation1[i][2] * this.objectPoints[0][2]);

			translation2[i] = translation[i] -
				(rotation2[i][0] * this.objectPoints[0][0] +
					rotation2[i][1] * this.objectPoints[0][1] +
					rotation2[i][2] * this.objectPoints[0][2]);
		}

		error1 = this.error(imagePoints, rotation1, translation1);
		error2 = this.error(imagePoints, rotation2, translation2);

		if ((error1.euclidean >= 0.0) && (error2.euclidean >= 0.0)) {
			if (error2.euclidean < error1.euclidean) {
				error = error2;
				for (i = 0; i < 3; ++i) {
					for (j = 0; j < 3; ++j) {
						rotation[i][j] = rotation2[i][j];
					}
				}
			} else {
				error = error1;
				for (i = 0; i < 3; ++i) {
					for (j = 0; j < 3; ++j) {
						rotation[i][j] = rotation1[i][j];
					}
				}
			}
		}

		if ((error1.euclidean < 0.0) && (error2.euclidean >= 0.0)) {
			error = error2;
			for (i = 0; i < 3; ++i) {
				for (j = 0; j < 3; ++j) {
					rotation[i][j] = rotation2[i][j];
				}
			}
		}

		if ((error2.euclidean < 0.0) && (error1.euclidean >= 0.0)) {
			error = error1;
			for (i = 0; i < 3; ++i) {
				for (j = 0; j < 3; ++j) {
					rotation[i][j] = rotation1[i][j];
				}
			}
		}

		for (i = 0; i < np; ++i) {
			factor = 0.0;
			for (j = 0; j < 3; ++j) {
				factor += this.objectVectors[i][j] * rotation[2][j] / translation[2];
			}
			sopImagePoints[i].x = (1.0 + factor) * imagePoints[i].x;
			sopImagePoints[i].y = (1.0 + factor) * imagePoints[i].y;
		}

		oldImageDifference = imageDifference;
		imageDifference = 0.0;

		for (i = 0; i < np; ++i) {
			imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x);
			imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y);
		}

		delta = Math.abs(imageDifference - oldImageDifference);

		converged = (0.0 === error.pixels) || (delta < 0.01);
	}

	return error;
};

POS.Posit.prototype.error = function (imagePoints, rotation, translation) {
	var np = this.objectPoints.length,
	move = [],
	projection = [],
	errorvec = [],
	euclidean = 0.0,
	pixels = 0.0,
	maximum = 0.0,
	i,
	j,
	k;

	if (!this.isValid(rotation, translation)) {
		return {
			euclidean : -1.0,
			pixels : -1,
			maximum : -1.0
		};
	}

	for (i = 0; i < np; ++i) {
		move[i] = [];
		for (j = 0; j < 3; ++j) {
			move[i][j] = translation[j];
		}
	}

	for (i = 0; i < np; ++i) {
		for (j = 0; j < 3; ++j) {
			for (k = 0; k < 3; ++k) {
				move[i][j] += rotation[j][k] * this.objectPoints[i][k];
			}
		}
	}

	for (i = 0; i < np; ++i) {
		projection[i] = [];
		for (j = 0; j < 2; ++j) {
			projection[i][j] = this.focalLength * move[i][j] / move[i][2];
		}
	}

	for (i = 0; i < np; ++i) {
		errorvec[i] = [projection[i][0] - imagePoints[i].x,
			projection[i][1] - imagePoints[i].y];
	}

	for (i = 0; i < np; ++i) {
		euclidean += Math.sqrt(errorvec[i][0] * errorvec[i][0] +
			errorvec[i][1] * errorvec[i][1]);

		pixels += Math.abs(Math.round(projection[i][0]) - Math.round(imagePoints[i].x)) +
		Math.abs(Math.round(projection[i][1]) - Math.round(imagePoints[i].y));

		if (Math.abs(errorvec[i][0]) > maximum) {
			maximum = Math.abs(errorvec[i][0]);
		}
		if (Math.abs(errorvec[i][1]) > maximum) {
			maximum = Math.abs(errorvec[i][1]);
		}
	}

	return {
		euclidean : euclidean / np,
		pixels : pixels,
		maximum : maximum
	};
};

POS.pseudoInverse = function (a, n, b) {
	var w = [],
	v = [[], [], []],
	s = [[], [], []],
	wmax = 0.0,
	cn = 0,
	i,
	j,
	k;

	require('./svd').svdcmp(a, n, 3, w, v);

	for (i = 0; i < 3; ++i) {
		if (w[i] > wmax) {
			wmax = w[i];
		}
	}

	wmax *= 0.01;

	for (i = 0; i < 3; ++i) {
		if (w[i] < wmax) {
			w[i] = 0.0;
		}
	}

	for (j = 0; j < 3; ++j) {
		if (0.0 === w[j]) {
			++cn;
			for (k = j; k < 2; ++k) {
				for (i = 0; i < n; ++i) {
					a[i][k] = a[i][k + 1];
				}
				for (i = 0; i < 3; ++i) {
					v[i][k] = v[i][k + 1];
				}
			}
		}
	}

	for (j = 0; j < 2; ++j) {
		if (0.0 === w[j]) {
			w[j] = w[j + 1];
		}
	}

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < 3 - cn; ++j) {
			s[i][j] = v[i][j] / w[j];
		}
	}

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < n; ++j) {
			b[i][j] = 0.0;
			for (k = 0; k < 3 - cn; ++k) {
				b[i][j] += s[i][k] * a[j][k];
			}
		}
	}
};

POS.Pose = function (error1, rotation1, translation1, error2, rotation2, translation2) {
	this.bestError = error1;
	this.bestRotation = rotation1;
	this.bestTranslation = translation1;
	this.alternativeError = error2;
	this.alternativeRotation = rotation2;
	this.alternativeTranslation = translation2;
};

module.exports = POS
